/**
  ******************************************************************************
  * @file    stepper_motor.c
  * @brief   串口步进电机控制协议驱动
  *          USART6
  ******************************************************************************
  */

#include "stepper_motor.h"
#include "usart.h"

/** @defgroup 内部计算BCC校验位(前九个字节的异或和)函数
  * @{
  */
static uint8_t CalcBCC(const uint8_t *buf, uint8_t len)
{
    uint8_t bcc = 0;
    for (uint8_t i = 0; i < len; i++)
        bcc ^= buf[i];
    return bcc;
}

/**
  * @brief  请求电机数据并启动接收
	* @param  motor_ID：默认为0x01，目前配置俯仰电机ID为0x01，底座电机ID为0x02
						STEP_ID1、STEP_ID2
*/
void Motor_RequestData(StepMotor_ID motor_ID)
{
    rx_index = 0;
    rx_complete = 0;
    
    // 发送请求命令
		if(motor_ID == 0x01)
		{
			HAL_UART_Transmit(&huart2, motor2_request, sizeof(motor1_request), HAL_MAX_DELAY);
		}
		else if(motor_ID == 0x02)
		{
			HAL_UART_Transmit(&huart2, motor2_request, sizeof(motor2_request), HAL_MAX_DELAY);
		}

    // 启动接收9字节数据
    HAL_UART_Receive_IT(&huart2, rx_buffer, 9);
}

// 串口接收完成回调
void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
{
    if(huart->Instance == USART2)
    {
        rx_complete = 1;  // 接收完成标志
    }
}

// 解析电机反馈数据
short Parse_MotorData(void)
{
		HAL_Delay(10);
    if(rx_complete)
    {
        uint8_t address = rx_buffer[0];           // 电机地址
        uint8_t status = rx_buffer[1];            // 状态位
        uint16_t speed = (rx_buffer[2] << 8) | rx_buffer[3];     // 速度
        uint32_t position = (rx_buffer[4] << 24) | (rx_buffer[5] << 16) | 
                           (rx_buffer[6] << 8) | rx_buffer[7];    // 位置
        uint8_t checksum = rx_buffer[8];          // 校验位
        
        // 处理数据
        printf("motor_ID: 0x%02X\n", address);
        printf("status: 0x%02X\n", status);
        printf("speed: %d\n", speed);
        printf("position: %d\n", position);
        
        rx_complete = 0;  // 清除标志
				return status;
    }
		else return 0;
}


/**
  * @brief  按协议封装并发送一帧数据
  * @param  frame 指向用户填充的结构体指针
  * @note   控制须严格按照此协议	
  */
void STEP_USART2_SendFrame(StepMotorFrame_t *frame)
{
    uint8_t tx[11];

    tx[0]  = 0x7B;               /* 帧头 */
		tx[1]  = frame->motor_num;   /* 控制 ID 默认0x01、目前配置俯仰电机ID为0x01，底座电机ID为0x02*/
    tx[2]  = frame->mode;        /* 控制模式0x01 为速度控制模式，0x02 为位置控制模式，0x03为力矩控制模式、0x04 为单圈绝对角度控制模式。*/
    tx[3]  = frame->dir; //0 为逆时针转动，1 为顺时针转动
    tx[4]  = frame->micro;       /* 细分值 */
    tx[5]  = (uint8_t)(frame->posH & 0xFF);
    tx[6]  = (uint8_t)(frame->posL & 0xFF);
    tx[7]  = (uint8_t)(frame->speedH & 0xFF);
    tx[8]  = (uint8_t)(frame->speedL & 0xFF);
    tx[9]  = CalcBCC(tx, 9);     /* BCC 校验 */
    tx[10] = 0x7D;               /* 帧尾 */
	
		HAL_UART_Transmit(&huart2, tx, sizeof(tx), HAL_MAX_DELAY);
}

/**
  * @brief  一步到位的电机控制函数
	* @param  num		 电机ID：默认为0x01，目前配置俯仰电机ID为0x01，底座电机ID为0x02
  * @param  mode   控制模式：STEP_MODE_SPEED / STEP_MODE_POSITION / STEP_MODE_TORQUE / STEP_MODE_ANGLE
  * @param  dir    旋转方向：0-逆时针 1-顺时针
  * @param  micro  细分数：2/4/8/16/32
  * @param  posVal 位置/角度/电流值（单位：角度*10 或 mA） 例如：0x03、0xe8 =0x03e8=1000/10.0=100 度。
  *                 SPEED 模式下填 0
  * @param  speed  转速值（单位：rad/s *10）o h
  *                 非 SPEED 模式填 0
  */
void STEP_MotorCtrl(uint8_t num, StepMode_t mode, uint8_t dir, uint8_t micro, int16_t posVal,int16_t speed)
{
    StepMotorFrame_t f;
		f.motor_num = num;
    f.mode   		= mode;
    f.dir    		= dir;
    f.micro  		= micro;

    /* 高低 8 位拆分 */
    f.posH   = (posVal >> 8) & 0xFF;
    f.posL   = posVal & 0xFF;

    f.speedH = (speed >> 8) & 0xFF;
    f.speedL = speed & 0xFF;

    STEP_USART2_SendFrame(&f);
		HAL_Delay(10);
}

/**
  * @brief  按协议封装并发送一帧数据
  * @param  frame 指向用户填充的结构体指针
  * @note   控制须严格按照此协议	
  */
void STEPMotor_Reset(void)
{
		STEP_MotorCtrl(0x02, STEP_MODE_ANGLE, 0x00, 0x20, 0, 100); //电机ID，电机模式，电机方向，电机细分，绝对角度*10，速度*10
		HAL_Delay(1000);
}

/**
  * @brief  在真正下发控制指令前，先主动查询一次电机状态，
  *         只有当返回的状态字节 status == 0x01 时才发控制帧。
  * @note   本函数会阻塞等待串口 9 字节接收完毕。
  *         如需非阻塞/超时，请使用 STEP_MotorCtrl_WithCheck_Timeout()。
  */
void STEP_MotorCtrl_WithCheck(StepMotor_ID num,
                              StepMode_t mode,
                              uint8_t dir,
                              uint8_t micro,
                              int16_t posVal,
                              int16_t speed)
{
    short status = 0;


		while(1)
		{
			 /* 1. 请求电机数据 */
			Motor_RequestData(num);
			
			/* 3. 解析并判断状态位 */
			status = Parse_MotorData();
			if (status == 0x01)
			{
					printf("OK");
					/* 4. 状态 OK，下发控制帧 */
					STEP_MotorCtrl(num, mode, dir, micro, posVal, speed);
					break;
			}
			/* 若 status != 0x01 则直接返回，不做任何动作 */
		}	
		
}
